#include <iostream>
#include <opencv2/core.hpp>
#include <opencv2/imgcodecs.hpp>
#include <opencv2/imgproc.hpp>

static int _non_equal_points(const cv::Mat& m1, const cv::Mat& m2) {
    int cn = m1.channels();
    CV_Assert(cn == 1 || cn ==2);
    CV_Assert(m2.channels() == cn);
    CV_Assert(m1.type() == CV_64FC1 || m1.type() == CV_64FC2);
    CV_Assert(m1.type() == m2.type());
    CV_Assert(m1.rows == m2.rows && m2.cols == m1.cols);
    
    std::vector<cv::Point> non_equal_points;
    for (int i = 0; i < m1.rows; i++) {
        for (int j = 0; j < m1.cols; j++) {
            if (cn == 1) {
                double d1 = m1.ptr<double>(i)[j];
                double d2 = m2.ptr<double>(i)[j];
                if (std::abs(d1-d2) > __DBL_EPSILON__) {
                    non_equal_points.push_back(cv::Point(j,i));
                }
            } else if (cn == 2) {
                cv::Vec2d v1 = m1.ptr<cv::Vec2d>(i)[j];
                cv::Vec2d v2 = m2.ptr<cv::Vec2d>(i)[j];
                for (int k = 0; k < 2; k++) {
                    double d1 = v1[k];
                    double d2 = v2[k];
                    if (std::abs(d1-d2) > __DBL_EPSILON__) {
                        non_equal_points.push_back(cv::Point(j, i));
                        break;
                    }
                }
            }
        }
    }
    return non_equal_points.size();
}

static void _mul_spectrum_2_channels(const cv::Mat& m1, const cv::Mat& m2, cv::Mat& dst, bool conjB) {
    CV_Assert(m1.channels() == 2);
    dst.create(m1.size(), m1.type());
    
    for (int i = 0; i < m1.rows; i++) {
        for (int j = 0; j < m1.cols; j++) {
            cv::Vec2d v1 = m1.ptr<cv::Vec2d>(i)[j];
            cv::Vec2d v2 = m2.ptr<cv::Vec2d>(i)[j];
            double re1 = v1[0], im1 = v1[1];
            double re2 = v2[0], im2 = v2[1];
            if (conjB) {
                im2 = -im2;
            }
            cv::Vec2d v;
            v[0] = re1*re2 - im1*im2;
            v[1] = re1*im2 + re2*im1;
            dst.ptr<cv::Vec2d>(i)[j] = v;
        }
    }
}

static void _mul_spectrum_1_channel(const cv::Mat& m1, const cv::Mat& m2, cv::Mat& dst, bool conjB) {
    CV_Assert(m1.channels() == 1);
    dst.create(m1.size(), m1.type());
    
    dst.ptr<double>(0)[0] = m1.ptr<double>(0)[0] * m2.ptr<double>(0)[0];
    for (int i = 1; i <= m1.rows-2; i+=2) {
        double re1 = m1.ptr<double>(i)[0];
        double im1 = m1.ptr<double>(i+1)[0];
        double re2 = m2.ptr<double>(i)[0];
        double im2 = m2.ptr<double>(i+1)[0];
        if (conjB) {
            im2 = -im2;
        }
        dst.ptr<double>(i)[0] = re1*re2 - im1*im2;
        dst.ptr<double>(i+1)[0] = re1*im2 + re2*im1;
    }
    if ((m1.rows & 1) == 0) {
        dst.ptr<double>(m1.rows-1)[0] = m1.ptr<double>(m1.rows-1)[0] * m2.ptr<double>(m1.rows-1)[0];
    }
    
    if ((m1.cols & 1) == 0) {
        dst.ptr<double>(0)[m1.cols-1] = m1.ptr<double>(0)[m1.cols-1] * m2.ptr<double>(0)[m1.cols-1];
        for (int i = 1; i <= m1.rows-2; i+=2) {
            double re1 = m1.ptr<double>(i)[m1.cols-1];
            double im1 = m1.ptr<double>(i+1)[m1.cols-1];
            double re2 = m2.ptr<double>(i)[m1.cols-1];
            double im2 = m2.ptr<double>(i+1)[m1.cols-1];
            if (conjB) {
                im2 = -im2;
            }
            dst.ptr<double>(i)[m1.cols-1] = re1*re2 - im1*im2;
            dst.ptr<double>(i+1)[m1.cols-1] = re1*im2 + re2*im1;
        }
        if ((m1.rows & 1) == 0) {
            dst.ptr<double>(m1.rows-1)[m1.cols-1] = m1.ptr<double>(m1.rows-1)[m1.cols-1] * m2.ptr<double>(m1.rows-1)[m1.cols-1];
        }
    }
    
    int j0 = 1;
    int j1 = m1.cols - ((m1.cols & 1) == 0 ? 1 : 0);
    for (int i = 0; i < m1.rows; i++) {
        for (int j = j0; j < j1; j += 2) {
            double re1 = m1.ptr<double>(i)[j];
            double im1 = m1.ptr<double>(i)[j+1];
            double re2 = m2.ptr<double>(i)[j];
            double im2 = m2.ptr<double>(i)[j+1];
            if (conjB) {
                im2 = -im2;
            }
            dst.ptr<double>(i)[j] = re1*re2 - im1*im2;
            dst.ptr<double>(i)[j+1] = re1*im2 + re2*im1;
        }
    }
}


int main(int argc, char **argv) {
    //cv::Mat mat1(100, 100, CV_64FC2);
    //cv::Mat mat2(100, 100, CV_64FC2);
    cv::Mat mat1(100, 100, CV_64FC1);
    cv::Mat mat2(100, 100, CV_64FC1);
    cv::randu(mat1, cv::Scalar(-1000), cv::Scalar(1000));
    cv::randu(mat2, cv::Scalar(-1000), cv::Scalar(1000));
    
    cv::Mat dst1, dst2;
    bool conjB = false;
    //cv::mulSpectrums(mat1, mat2, dst1, 0, conjB);
    //_mul_spectrum_2_channels(mat1, mat2, dst2, conjB);
    //cv::mulSpectrums(mat1, mat2, dst1, 0, conjB);
    //_mul_spectrum_1_channel(mat1, mat2, dst2, conjB);
    
    conjB = true;
    //cv::mulSpectrums(mat1, mat2, dst1, 0, conjB);
    //_mul_spectrum_2_channels(mat1, mat2, dst2, conjB);
    cv::mulSpectrums(mat1, mat2, dst1, 0, conjB);
    _mul_spectrum_1_channel(mat1, mat2, dst2, conjB);
    
    int non_equals = _non_equal_points(dst1, dst2);
    std::cout << "not equals points numbers = " << non_equals << std::endl;
    std::cout << "results are " << (non_equals == 0 ? "same" : "not same") << std::endl;
    
    return 0;
}
